%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Edited by Shu Jiang, 2011.
% ALL RIGHTS ARE RESERVED.
% mailto: shujiang@tamu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate the slope from the COM to swing leg ankle
function ret = cal_COM_3D(name)
addpath('~/Documents/Research_Related/COM_HD/')
addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/')
addpath('~/Documents/Research_Related/COM_HD/human_data_analysis/humanData')
addpath('/home/shu/Documents/Research_Related/Data')


    humanData = getData(name);
    a = humanData.sns_brk(1);
    b = humanData.sns_brk(2);
    c = humanData.sns_brk(3);
    disp(b)
    disp(c)
    d = humanData.sns_brk(4);
    %Extract x & y values from humanData
%     if i<= 6
        [xpos,ypos,zpos] = getPosCartesian(humanData);
%     else 
%         [xpos,ypos,zpos] = getPosCartesianNew(humanData);
%     end
%     [xpos,ypos,zpos] = getPosCartesian(humanData);

%     offset = humanData.sns_brk(2);
%     xpos.hip_avg = xpos.hip_avg-xpos. rankle_avg(offset);
%     xpos.rheel = xpos.rheel - xpos. rankle_avg(offset);
%     xpos.rtoe = xpos.rtoe - xpos. rankle_avg(offset);
%     xpos.lheel = xpos.lheel - xpos. rankle_avg(offset);
%     xpos.ltoe = xpos.ltoe - xpos. rankle_avg(offset);
%     xpos.rknee = xpos.rknee-xpos. rankle_avg(offset);
%     xpos.lknee = xpos.lknee-xpos. rankle_avg(offset);
%     xpos.rhip = xpos.rhip - xpos. rankle_avg(offset);
%     xpos.lhip = xpos.lhip - xpos. rankle_avg(offset);
%     xpos.torso = xpos.torso - xpos. rankle_avg(offset);
%     xpos.rankle_avg = xpos.rankle_avg - xpos. rankle_avg(offset);
%     xpos.lankle_avg = xpos.lankle_avg - xpos. rankle_avg(offset);


%Calculate human parameters
mass = calcMassDist(humanData.weight);

% from heel strike to the other heel strike
% time_len = c-b+1;
%%
for i = 1:length(xpos.lheel)
% i = j+b-1;
[lfc_x,lfc_y,lfc_z] = calcFootCenter_BothLeg(xpos.lankle_avg(i),xpos.ltoe(i),xpos.lheel(i)...
    ,ypos.lankle_avg(i),ypos.ltoe(i),ypos.lheel(i),...
    zpos.lankle_avg(i),zpos.ltoe(i),zpos.lheel(i));

[rfc_x,rfc_y,rfc_z] = calcFootCenter_BothLeg(xpos.rankle_avg(i),xpos.rtoe(i),xpos.rheel(i)...
    ,ypos.rankle_avg(i),ypos.rtoe(i),ypos.rheel(i),...
    zpos.rankle_avg(i),zpos.rtoe(i),zpos.rheel(i));


% calculate the CoM from human data
alpha1 = 0.433;
alpha2 = (1-0.433);
beta1 = 1;
beta2 = 0;

com_x(i) = (mass.torso*(beta1*xpos.torso(i)+beta2*xpos.hip_avg(i))+...
    mass.thigh*(alpha2*xpos.rhip(i)+alpha1*xpos.rknee(i))...
    +mass.thigh*(alpha2*xpos.lhip(i)+alpha1*xpos.lknee(i))...
    +mass.calf*(alpha2*xpos.rknee(i)+alpha1*xpos.rankle_avg(i))...
    +mass.calf*(alpha2*xpos.lknee(i)+alpha1*xpos.lankle_avg(i))...
    +mass.foot*rfc_x+mass.foot*lfc_x...
    )/(mass.torso+2*mass.thigh+2*mass.calf+2*mass.foot);

com_y(i) = (mass.torso*(beta1*ypos.torso(i)+beta2*ypos.hip_avg(i))+...
    mass.thigh*(alpha2*ypos.rhip(i)+alpha1*ypos.rknee(i))...
    +mass.thigh*(alpha2*ypos.lhip(i)+alpha1*ypos.lknee(i))...
    +mass.calf*(alpha2*ypos.rknee(i)+alpha1*ypos.rankle_avg(i))...
    +mass.calf*(alpha2*ypos.lknee(i)+alpha1*ypos.lankle_avg(i))+...
    mass.foot*rfc_y+mass.foot*lfc_y...
    )/(mass.torso+2*mass.thigh+2*mass.calf+2*mass.foot);


com_z(i) = (mass.torso*(beta1*zpos.torso(i)+beta2*zpos.hip_avg(i))+...
    mass.thigh*(alpha2*zpos.rhip(i)+alpha1*zpos.rknee(i))...
    +mass.thigh*(alpha2*zpos.lhip(i)+alpha1*zpos.lknee(i))...
    +mass.calf*(alpha2*zpos.rknee(i)+alpha1*zpos.rankle_avg(i))...
    +mass.calf*(alpha2*zpos.lknee(i)+alpha1*zpos.lankle_avg(i))+...
    mass.foot*rfc_z+mass.foot*lfc_z...
    )/(mass.torso+2*mass.thigh+2*mass.calf+2*mass.foot); % lfc_z


% swing ankle
wpt_x(i) = xpos.lankle_avg(i);
wpt_y(i) = ypos.lankle_avg(i);
% 
% 
% % calculate the length
com_len(i) = pdist([com_x(i),com_y(i);wpt_x(i),wpt_y(i)]);
% com_len2(i)=sqrt((com_x(i)-spt_x(i)).^2+(com_y(i)-spt_y(i)).^2);
% 
% % calculate the angle
com_slope(i) = (com_x(i)-wpt_x(i))/(com_y(i)-wpt_y(i));
end
% plot(com_slope(b:c))
% hold on ;
ret.slope = com_slope(b:c);
ret.zpos = com_z;
% plot(wpt_x(b:c),wpt_y(b:c),'k');
% hold on
% plot(com_slope(b:c),'r');
% hold on;
% plot(wpt_x,wpt_y)
end

function [L_fc_x,L_fc_y,L_fc_z] = calcFootCenter_BothLeg(xankle,xtoe,xheel,yankle,ytoe,yheel...
    ,zankle,ztoe,zheel)

L_fc_x = 1/3*(xankle+xtoe+xheel) ;
L_fc_y = 1/3*(yankle+ytoe+yheel) ;
L_fc_z = 1/3*(zankle+ztoe+zheel) ;
end